#include "full_cover_path_planner/loop_planner.h"
#include "path_smooth/points_process.h"

// using namespace global_path_planner::path_smooth;
using namespace global_path_planner::path_smooth;

namespace global_path_planner
{
    namespace ccpp
    {
        LoopPlanner::LoopPlanner(std::shared_ptr<MapProcess>& map_process,
                                std::shared_ptr<CellDecomposer>& cell_decomp,
                                std::shared_ptr<Polygon2D>& poly2d)
                                : __map_Proc_Ptr(map_process),
                                __polygon2d_Ptr(poly2d),
                                __cell_Decomp_Ptr(cell_decomp)
        {
            // __polygon2d_Ptr = std::make_shared<Polygon2D>(Polygon2D());
            // __cell_Decomp_Ptr = std::make_shared<CellDecomposer>(CellDecomposer(__polygon2d_Ptr));
        }

        LoopPlanner::~LoopPlanner()
        {
            // if (__cell_Decomposer)
            //     delete __cell_Decomposer;
        }

        // 贴边路径
        std::vector<std::vector<cv::Point>>
        LoopPlanner::getPerimPaths(cv::Mat &img, int &narrow_lane,
                                   float &sweep_width, float &sweep_interval,
                                   std::vector<std::vector<cv::Point>> all_polygons)
        {
            std::string output_dir = "/home/xxty/trobot/trobot_ros/ccpp_ws/src/global_path_planner/output/";

            std::vector<std::vector<cv::Point>> result_optimized_perim;
            auto sweeper_Width = sweep_width * 2; // sweep_width: 路径间距
            // 优化轮廓
            if (cv::countNonZero(img) && all_polygons.size() > 0)
            {
                cv::imwrite(output_dir + "[getPerimPaths]_001_origin.png", img);
                result_optimized_perim = getOptimizedPaths(2, img, all_polygons);
                cv::imwrite(output_dir + "[getPerimPaths]_002_getOptimizedPaths.png", img);

                // 优化后的轮廓覆盖之前的轮廓
                if (result_optimized_perim.size() > 0)
                {
                    img = __map_Proc_Ptr->MaskAreaImage(img, result_optimized_perim);
                    cv::imwrite(output_dir + "[getPerimPaths]_003.png", img);
                }
                
                // 获取膨胀半径，即机器人半径
                int sz = std::round(sweeper_Width / sweep_interval);
                // std::cout << "sz:" << sz << std::endl;
                if (narrow_lane == 1)
                    sz = 2;
                cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
                cv::erode(img, img, erode_kernel); // 对img的修改是 贴边+弓字/回字 的后续操作需要用到
                cv::imwrite(output_dir + "[getPerimPaths]_004_erode.png", img);
                // 形态学闭操作, 去除狭窄白色区域 (避免findcontours内外轮廓有重合像素)
                cv::Mat open_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3), cv::Point(-1, -1));
                cv::morphologyEx(img, img, cv::MORPH_OPEN, open_kernel);
                cv::imwrite(output_dir + "[getPerimPaths]_005_morphologyEx.png", img);
            }
            return result_optimized_perim;
        }

        // 获取回字形/贴边
        std::vector<std::vector<cv::Point>> LoopPlanner::getWrapperPath(cv::Mat &img, int perim, float &sweep_width,
                                                                        float &sweep_interval, float &sweep_offset,
                                                                        bool &contain_circle, bool &loop_smooth,
                                                                        std::vector<std::vector<cv::Point>> &polys)
        {
            // perim: 1
            std::vector<std::vector<cv::Point>> optimized_paths;
            cv::Mat update_img = img.clone();
            std::vector<std::vector<cv::Point>> tmp_polys = polys;
            auto sweeper_Width = sweep_width * 2 + sweep_offset;

            // 处理内部的轮廓
            while (cv::countNonZero(update_img) && tmp_polys.size() > 0)
            {
                std::vector<std::vector<cv::Point>> wrap_pts;
                wrap_pts = getOptimizedPaths(1, update_img, tmp_polys);
                optimized_paths.insert(optimized_paths.end(), wrap_pts.begin(), wrap_pts.end());

                // 获取膨胀半径，即机器人半径
                int sz = std::round(sweeper_Width / sweep_interval);
                cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(sz, sz), cv::Point(-1, -1)); // size: robot radius
                cv::erode(update_img, update_img, erode_kernel);

                // 提取腐蚀后的轮廓
                bool loop = true;
                auto tmp_all_polys = __map_Proc_Ptr->getAllContours(update_img, loop);
                tmp_polys.clear();
                for (auto poly : tmp_all_polys)
                {
                    for (auto it = poly.second.second.begin();
                         it != poly.second.second.end(); it++)
                    {
                        tmp_polys.emplace_back(*it);
                    }
                }
            }
            return optimized_paths;
        }

        // 优化轨迹
        std::vector<std::vector<cv::Point>>
        LoopPlanner::getOptimizedPaths(int perim, cv::Mat &img,
                                       std::vector<std::vector<cv::Point>> pts)
        {
            // perim: 1-回字, 2-贴边
            // 增加致密点
            PathSmoother path_smoother;
            auto tmpts = getDensePoints(perim, path_smoother, pts);
            std::vector<std::vector<common::Vec2d>> densePts;
            std::vector<std::vector<cv::Point>> res_densePts;
            for (auto i = 0; i < tmpts.size(); i++)
            {
                std::vector<common::Vec2d> v2d;
                for (auto j = 0; j < tmpts[i].size(); j++)
                {
                    v2d.push_back(common::Vec2d(tmpts[i][j].x, tmpts[i][j].y));
                }
                densePts.push_back(v2d);
            }
            // 平滑处理
            // for (auto &p : densePts)
            // {
            //     path_smoother = PathSmoother(img, p);
            //     if (perim == 0)
            //         path_smoother.setParams(0.1, 0.2, 0.5, 3);
            //     else if (perim == 1)
            //         path_smoother.setParams(0.05, 1.0, 1.5, 5);
            //     // else pathSmoother.setParams(0.05, 0.1, 0.2, 100);
            //     else
            //         path_smoother.setParams(0.1, 0.5, 2.0, 15);
            //     path_smoother.smoothPath(perim);
            //     p = path_smoother.getSmoothedPath();
            // }
            // vec2d转化为cv::Point
            for (auto i = 0; i < densePts.size(); i++)
            {
                std::vector<cv::Point> cv_pts;
                for (auto j = 0; j < densePts[i].size(); j++)
                {
                    cv_pts.push_back(cv::Point(densePts[i][j].x(), densePts[i][j].y()));
                }
                res_densePts.push_back(cv_pts);
            }
            return res_densePts;
        }

		std::vector<std::vector<cv::Point>> LoopPlanner::getDensePoints(int perim, PathSmoother& path_smoother,
                                                                       std::vector<std::vector<cv::Point>> sparsePaths)
		{
			std::vector<std::vector<cv::Point>> densePaths;
			if (sparsePaths.empty())
				return densePaths;

            std::vector<std::vector<std::tuple<double, double, double>>> tuple_Sparse_Paths;
            for(auto i = 0; i < sparsePaths.size(); i++){
                // cv::Point转化为tuple
                std::vector<std::tuple<double, double, double>> tuple_paths;
                for(auto j = 0; j < sparsePaths[i].size(); j++){
                    std::tuple<double, double, double> start = std::make_tuple(sparsePaths[i][j].x, 
                                                                                sparsePaths[i][j].y,
                                                                                0.);
                    std::tuple<double, double, double> end;
                    if(perim != 0 && j == sparsePaths[i].size() - 1){
                        end = std::make_tuple(sparsePaths[i][0].x, 
                                            sparsePaths[i][0].y,
                                            0.);
                    }else if(perim == 0 && j == sparsePaths[i].size() - 1){
                        continue;
                    }else{
                        end = std::make_tuple(sparsePaths[i][j + 1].x, 
                                            sparsePaths[i][j + 1].y,
                                            0.);
                    }
                    // 两点之间间隔interval增加致密点
                    std::vector<std::tuple<double, double, double>> tuple_path;
                    tuple_path = points::AddDensePoints(start, end, 2);
			        // if (perim == 2)
			        // 	tuple_path = points::AddDensePoints(start, end, 2);
			        // else if (perim == 1)
			        // 	tuple_path = points::AddDensePoints(start, end, 10);
			        // else
			        // 	tuple_path = points::AddDensePoints(start, end, 4);
                    tuple_paths.insert(tuple_paths.end(), tuple_path.begin(), tuple_path.end());
                }
                // tuple转化为cv::Point
                std::vector<cv::Point> dense_path;
                for(auto j = 0; j < tuple_paths.size(); j++){
                    dense_path.emplace_back(cv::Point(std::get<0>(tuple_paths[j]), std::get<1>(tuple_paths[j])));
                }
                densePaths.emplace_back(dense_path);
            }

			return densePaths;
		}

        // 根据起点对所有回字形进行排序
        std::vector<std::vector<cv::Point>>
        LoopPlanner::sortAllLoopPaths(Point_2 &start,
                                      std::unordered_map<int, std::vector<std::vector<cv::Point>>> origin_loop_paths)
        {
            // point_2 start转化为 cv::Point start
            cv::Point cv_start(CGAL::to_double(start.x()), CGAL::to_double(start.y()));
            std::vector<std::vector<cv::Point>> res_loop_paths;
            // 删除为空的点集
            for (auto iter = origin_loop_paths.begin(); iter != origin_loop_paths.end();)
            {
                if (iter->second.size() <= 0)
                {
                    iter = origin_loop_paths.erase(iter);
                    continue;
                }
                iter++;
            }
            // 根据起点进行排序
            while (origin_loop_paths.size() > 0)
            {
                // 根据起点到单个cell的最小距离（分别计算最外层和最内层）
                double dist_Min = DBL_MAX;
                std::unordered_map<int, std::vector<std::vector<cv::Point>>>::iterator iter_idx;
                std::vector<cv::Point>::iterator it_idx;
                bool out_inner = true;
                // std::cout << " origin_loop_paths.size:" << origin_loop_paths.size() << std::endl;
                for (auto iter = origin_loop_paths.begin(); iter != origin_loop_paths.end();)
                {
                    // 起点到最外层距离
                    for (auto it = iter->second.front().begin(); it != iter->second.front().end(); it++)
                    {
                        double dx = cv_start.x - it->x;
                        double dy = cv_start.y - it->y;
                        if (std::sqrt(dx * dx + dy * dy) < dist_Min)
                        {
                            dist_Min = std::sqrt(dx * dx + dy * dy);
                            iter_idx = iter;
                            it_idx = it;
                            out_inner = true;
                        }
                    }
                    // 起点到最内层距离
                    for (auto it = iter->second.back().begin(); it != iter->second.back().end(); it++)
                    {
                        double dx = cv_start.x - it->x;
                        double dy = cv_start.y - it->y;
                        if (std::sqrt(dx * dx + dy * dy) < dist_Min)
                        {
                            dist_Min = std::sqrt(dx * dx + dy * dy);
                            iter_idx = iter;
                            it_idx = it;
                            out_inner = false;
                        }
                    }
                    iter++;
                }
                // std::cout << "out_inner:" << out_inner << std::endl;
                // 起点离最内层最近的，需要调整路径顺序
                if (!out_inner)
                {
                    std::reverse(iter_idx->second.begin(), iter_idx->second.end());
                }
                // 更新路径的起点
                cv_start.x = it_idx->x;
                cv_start.y = it_idx->y;
                auto sort_paths = sortLoopPaths(cv_start, iter_idx->second);
                res_loop_paths.insert(res_loop_paths.end(), sort_paths.begin(), sort_paths.end());
                origin_loop_paths.erase(iter_idx);
                cv_start.x = sort_paths.back().back().x;
                cv_start.y = sort_paths.back().back().y;
            }
            return res_loop_paths;
        }

        std::vector<std::vector<cv::Point>>
        LoopPlanner::sortLoopPaths(cv::Point &start, std::vector<std::vector<cv::Point>> loop_paths)
        {
            std::vector<std::vector<cv::Point>> sortPaths;
            std::vector<cv::Point> sortPath;
            double dist_Min = DBL_MAX;
            // 内部轮廓进行调准
            while (loop_paths.size() > 0)
            {
                dist_Min = DBL_MAX;
                sortPath.clear();
                std::vector<std::vector<cv::Point>>::iterator all_iter = loop_paths.begin();
                std::vector<cv::Point>::iterator iter = loop_paths[0].end();
                int dist_idx = -1;
                for (auto i = 0; i < loop_paths.size(); i++)
                {
                    for (auto it = loop_paths[i].begin(); it != loop_paths[i].end(); it++)
                    {
                        double dx = start.x - it->x;
                        double dy = start.y - it->y;
                        if (std::sqrt(dx * dx + dy * dy) < dist_Min)
                        {
                            dist_Min = std::sqrt(dx * dx + dy * dy);
                            iter = it;
                            dist_idx = i;
                        }
                    }
                }
                sortPath.insert(sortPath.end(), iter, loop_paths[dist_idx].end());
                sortPath.insert(sortPath.end(), loop_paths[dist_idx].begin(), iter);
                sortPath.push_back(sortPath[0]);
                sortPaths.push_back(sortPath);
                loop_paths.erase(all_iter + dist_idx);
                start.x = sortPath.back().x;
                start.y = sortPath.back().y;
            }
            return sortPaths;
        }

    } // namespace ccpp
} // namespace global_path_planner